classdef AllanVarianceTest < matlab.unittest.TestCase
    
    methods (Test)
        function vldAllanVariance(testCase)
            % 验证Allan方差算法 % TODO: 多正弦信号测试，增加判据
            
            %% 常量
            cst.sec2Rad = pi / 3600 / 180;
            cst.deg2Rad = pi / 180;
            cst_TG = load('earthconstants_virtual.mat', 'omegaIE', 'RE', 'eSq', 'f');
            
            %% 配置
            cfg_TG.IMU.enableErr = true;
            cfg_TG.IMU.enableNoise = true;
            
            %% 输入
            in_TG.genFuncName = 'gentgi_static';
            in_TG.genFuncInArg = {3600*4};
            
            %% 参量
            par_TG.samplePeriod = 0.02;
            par_TG.g = 0; % 重力加速度，m/s^2
            
            % 加速度计随机误差
            par_TG.IMU.acc.noise.R = [1e-3 0 0]'; % 单位m/s^3
            par_TG.IMU.acc.noise.K = [0 1e-3 0]'; % 单位m/s^(5/2)
            par_TG.IMU.acc.noise.N = [0 0 1e-3]'; % 单位m/s^(3/2)
            par_TG.IMU.acc.noise.Q = realmin * ones(3, 1); % 单位m/s
            par_TG.IMU.acc.noise.Omega0 = 0 * ones(3, 1); % 单位m/s^2
            par_TG.IMU.acc.noise.f0 = ones(3, 1); % 单位Hz
            
            % 陀螺随机误差
            par_TG.IMU.gyro.noise.R = [0 1e-4 1e-3]'; % 单位rad/s^2
            par_TG.IMU.gyro.noise.K = [0 1e-3 1e-4]'; % 位rad/s^(3/2)
            par_TG.IMU.gyro.noise.N = [0 1e-3 1e-4]'; % 单位rad/sqrt(s)
            par_TG.IMU.gyro.noise.Q = [1e-3 1e-4 1e-3]'; % 单位rad
            par_TG.IMU.gyro.noise.Omega0 = [1e-1 1e-2 1e-3]'; % 单位rad/s
            par_TG.IMU.gyro.noise.f0 = [10 3.3 1]'; % 单位Hz
            
            %% 初始条件
            iniCon_TG.CVG = eye(3); % 初始姿态矩阵
            iniCon_TG.CBV = eye(3); % 初始姿态矩阵
            iniCon_TG.vG = zeros(3, 1); % 初始速度
            iniCon_TG.pos = [30 * cst.deg2Rad, 110 * cst.deg2Rad, 0]'; % 纬度、经度、高度
            
            %% 运行仿真
            tic;
            [out_TG, auxOut_TG] = gentrajectory(in_TG, cfg_TG, par_TG, iniCon_TG, cst_TG);
            toc;
            save('allanvarnoise.mat', 'out_TG', 'auxOut_TG');
            
            %% 计算Allan方差，采用器件增量输出
            tmp.RRef = [[par_TG.IMU.acc.noise.R' par_TG.IMU.gyro.noise.R']; [par_TG.IMU.acc.noise.K' par_TG.IMU.gyro.noise.K']; zeros(1, 6); ...
                [par_TG.IMU.acc.noise.N' par_TG.IMU.gyro.noise.N']; [par_TG.IMU.acc.noise.Q' par_TG.IMU.gyro.noise.Q']; [par_TG.IMU.acc.noise.Omega0' par_TG.IMU.gyro.noise.Omega0']];
            % 最小二乘法
            [ out_AVIncLS.R, out_AVIncLS.K, out_AVIncLS.B, out_AVIncLS.N, out_AVIncLS.Q, out_AVIncLS.Omega0, out_AVIncLS.sigmaSq, out_AVIncLS.tau, out_AVIncLS.percentageError, out_AVIncLS.normFitErrRMS ] ...
                = allanvariance([out_TG.IMU.accOut, out_TG.IMU.gyroOut], par_TG.samplePeriod, ones(1, 6), [true(1, 3) false(1, 3)], ...
                'sinFreq', num2cell([par_TG.IMU.acc.noise.f0; par_TG.IMU.gyro.noise.f0]'), 'fitMethod', 1, 'resultIsInConvUnit', false);
            tmp.RCmp = [out_AVIncLS.R; out_AVIncLS.K; out_AVIncLS.B; out_AVIncLS.N; out_AVIncLS.Q; cell2mat(out_AVIncLS.Omega0)];
            out.RDiffIncLS = tmp.RRef - tmp.RCmp;
            out.RRelatPercentDiffIncLS = relatdiff(tmp.RCmp, tmp.RRef)*100;
            % LSNE法
            [ out_AVIncLSNE.R, out_AVIncLSNE.K, out_AVIncLSNE.B, out_AVIncLSNE.N, out_AVIncLSNE.Q, out_AVIncLSNE.Omega0, out_AVIncLSNE.sigmaSq, out_AVIncLSNE.tau, out_AVIncLSNE.percentageError, out_AVIncLSNE.normFitErrRMS ] ...
                = allanvariance([out_TG.IMU.accOut, out_TG.IMU.gyroOut], par_TG.samplePeriod, ones(1, 6), [true(1, 3) false(1, 3)], ...
                'sinFreq', num2cell([par_TG.IMU.acc.noise.f0; par_TG.IMU.gyro.noise.f0]'), 'fitMethod', 2, 'resultIsInConvUnit', false);
            tmp.RCmp = [out_AVIncLSNE.R; out_AVIncLSNE.K; out_AVIncLSNE.B; out_AVIncLSNE.N; out_AVIncLSNE.Q; cell2mat(out_AVIncLSNE.Omega0)];
            out.RDiffIncLSNE = tmp.RRef - tmp.RCmp;
            out.RRelatPercentDiffIncLSNE = relatdiff(tmp.RCmp, tmp.RRef)*100;
            
            %% 计算Allan方差，采用器件瞬时值输出（没有量化误差）
            tmp.RRef = [[par_TG.IMU.acc.noise.R' par_TG.IMU.gyro.noise.R']; [par_TG.IMU.acc.noise.K' par_TG.IMU.gyro.noise.K']; zeros(1, 6); ...
                [par_TG.IMU.acc.noise.N' par_TG.IMU.gyro.noise.N']; zeros(1, 6); [par_TG.IMU.acc.noise.Omega0' par_TG.IMU.gyro.noise.Omega0']];
            % 最小二乘法
            [ out_AVInstLS.R, out_AVInstLS.K, out_AVInstLS.B, out_AVInstLS.N, out_AVInstLS.Q, out_AVInstLS.Omega0, out_AVInstLS.sigmaSq, out_AVInstLS.tau, out_AVInstLS.percentageError, out_AVInstLS.normFitErrRMS ] ...
                = allanvariance([out_TG.IMU.fB, out_TG.IMU.omegaIBB], par_TG.samplePeriod, NaN(1, 6), [true(1, 3) false(1, 3)], ...
                'sensOutType', 2, 'sinFreq', num2cell([par_TG.IMU.acc.noise.f0; par_TG.IMU.gyro.noise.f0]'), 'fitMethod', 1, 'resultIsInConvUnit', false);
            tmp.RCmp = [out_AVInstLS.R; out_AVInstLS.K; out_AVInstLS.B; out_AVInstLS.N; out_AVInstLS.Q; cell2mat(out_AVInstLS.Omega0)];
            out.RDiffInstLS = tmp.RRef - tmp.RCmp;
            out.RRelatPercentDiffInstLS = relatdiff(tmp.RCmp, tmp.RRef)*100;
            % LSNE法
            [ out_AVInstLSNE.R, out_AVInstLSNE.K, out_AVInstLSNE.B, out_AVInstLSNE.N, out_AVInstLSNE.Q, out_AVInstLSNE.Omega0, out_AVInstLSNE.sigmaSq, out_AVInstLSNE.tau, out_AVInstLSNE.percentageError, out_AVInstLSNE.normFitErrRMS ] ...
                = allanvariance([out_TG.IMU.fB, out_TG.IMU.omegaIBB], par_TG.samplePeriod, NaN(1, 6), [true(1, 3) false(1, 3)], ...
                'sensOutType', 2, 'sinFreq', num2cell([par_TG.IMU.acc.noise.f0; par_TG.IMU.gyro.noise.f0]'), 'fitMethod', 2, 'resultIsInConvUnit', false);
            tmp.RCmp = [out_AVInstLSNE.R; out_AVInstLSNE.K; out_AVInstLSNE.B; out_AVInstLSNE.N; out_AVInstLSNE.Q; cell2mat(out_AVInstLSNE.Omega0)];
            out.RDiffInstLSNE = tmp.RRef - tmp.RCmp;
            out.RRelatPercentDiffInstLSNE = relatdiff(tmp.RCmp, tmp.RRef)*100;
            
            %% 后处理
            format shortG;
            disp('采用器件增量输出时最小二乘法拟合误差');
            disp(out.RDiffIncLS);
            disp('采用器件增量输出时最小二乘法拟合相对误差(%)');
            disp(out.RRelatPercentDiffIncLS);
            disp('采用器件增量输出时LSNE法拟合误差');
            disp(out.RDiffIncLSNE);
            disp('采用器件增量输出时LSNE法拟合相对误差(%)');
            disp(out.RRelatPercentDiffIncLSNE);
            disp('采用器件瞬时值输出时最小二乘法拟合误差');
            disp(out.RDiffInstLS);
            disp('采用器件瞬时值输出时最小二乘法拟合相对误差(%)');
            disp(out.RRelatPercentDiffInstLS);
            disp('采用器件瞬时值输出时LSNE法拟合误差');
            disp(out.RDiffInstLSNE);
            disp('采用器件瞬时值输出时LSNE法拟合相对误差(%)');
            disp(out.RRelatPercentDiffInstLSNE);
        end
        
        function vldAllanVarvianceAgainstAllanVar(testCase)
            % 验证与MATLAB Navigation Toolbox的allanvar函数结果的一致性
            
            % 数据生成
            bandWidth = 50; % imuSensor生成随机漂移的采样频率实际为奈奎斯特频率
            accParams = accelparams('NoiseDensity', [1 0 0], 'BiasInstability', [0 1 0], 'RandomWalk', [0 0 1]);
            gyroParams = gyroparams('NoiseDensity', [1 0 0], 'BiasInstability', [0 1 0], 'RandomWalk', [0 0 1]);
            IMU = imuSensor('Accelerometer', accParams, 'Gyroscope', gyroParams, 'SampleRate', 2*bandWidth);
            nSamp = 10000 * IMU.SampleRate; % 10000s
            [accelReadings, gyroReadings] = IMU(zeros(nSamp, 3), zeros(nSamp, 3));
            
            % 使用allanvariance函数计算
            [R, K, B, N, Q, ~, sigmaSq, tau0] = allanvariance([accelReadings, gyroReadings], 1/bandWidth, ones(1, 6), [true(1, 3) false(1, 3)], 'sensOutType', 2, 'resultIsInConvUnit', false, 'doRepeatIdentTauPts', false);
            disp('R K B N Q');
            disp(num2str([R; K; B; N; Q]'));
            
            % 使用allanvar函数在同样的tau上计算
            [avar, tau] = allanvar([accelReadings, gyroReadings], round(tau0*bandWidth), bandWidth);
            
            % 比较计算结果
            tauDiffAbsMax = max(abs(tau0 - tau));
            sigmaSqDiffAbsMax = max(abs(sigmaSq - avar));
            sigmaSqRelatDiffAbsMax = max(abs(relatdiff(sigmaSq, avar)));
            disp(['τ差值绝对值最大值（s）：' num2str(tauDiffAbsMax)]);
            disp(['Allan方差差值绝对值最大值：' num2str(sigmaSqDiffAbsMax)]);
            disp(['Allan方差相对差值绝对值最大值：' num2str(sigmaSqRelatDiffAbsMax)]);
            testCase.verifyLessThanOrEqual(tauDiffAbsMax, 4e-12);
            testCase.verifyLessThanOrEqual(sigmaSqDiffAbsMax, 2e-9);
            testCase.verifyLessThanOrEqual(sigmaSqRelatDiffAbsMax, 2e-10);
        end
    end
end